﻿using System;
using System.Collections.Generic;
using System.Text;
using System.Drawing;
using AForge.Imaging.Filters;
using AForge.Imaging;
using System.Drawing.Imaging;
using RoverStudio.Common.PositionSource;
using System.Runtime.CompilerServices;

namespace Position.CircleTracker
{
    public class RGCircleTracker
    {
        private Bitmap overlayImage = null;

        public Bitmap OverlayImage
        {
            get { return overlayImage; }
            set { overlayImage = value; }
        }



        private byte threshold = 110;

        public byte Threshold
        {
            get { return threshold; }
            set { threshold = value; }
        }


        private int minBlobSize = 10;

        public int MinBlobSize
        {
            get { return minBlobSize; }
            set { minBlobSize = value; }
        }
        private int maxBlobSize = 25;

        public int MaxBlobSize
        {
            get { return maxBlobSize; }
            set { maxBlobSize = value; }
        }


        private int left;

        public int Left
        {
            get { return this.left; }
            set { this.left = Math.Min(Math.Max(value, 0), 640); }
        }

        private int top;
        public int Top
        {
            get { return this.top; }
            set { this.top = Math.Min(Math.Max(value, 0), 480); }
        }

        private int width;
        public int Width
        {
            get { return this.width; }
            set { this.width = Math.Min(value, 640 - this.left); }
        }

        private int height;
        public int Height
        {
            get { return this.height; }
            set { this.height = Math.Min(value, 480 - this.top); }

        }



        public RGCircleTracker()
        {
            this.left = 0;
            this.top = 0;
            this.height = 480;
            this.width = 640;


 
 
        }


        public void Init(Bitmap bitmap)
        {
        }

//        private Bitmap overlayImage = new Bitmap(@"c:\img.bmp");
//        private Subtract subtract;



        private static double RELATIVE_THETA_CIRCLE = 0.5 / Math.PI;
        private static double RELATIVE_THETA_INV_CIRCLE = 0.97 * 0.5 / Math.PI;
        private static double SMALL_LARGE_CIRCLE_RADIUS_RATIO = 0.27397260273972601;

//        private static int counter = 10;

        private volatile bool lockSearchWindow = false;

        public bool LockSearchWindow
        {
            get { return lockSearchWindow; }
            set { lockSearchWindow = value; }
        }


        public void GetPositionAndDirection(ref Bitmap bitmap, out int x, out int y, out double theta, out Bitmap[] filteredImages)
        {

            //this.subtract = new Subtract(this.overlayImage);
            //Bitmap backgroundFiltered = this.subtract.Apply(bitmap);
            //backgroundFiltered.Save(@"c:\bckimg.bmp");
 //           this.overlayImage = bitmap;

            //bitmap = new Bitmap(@"D:\Projects\RoverStudio\LEDs\20090405\kep010_detektalas.bmp");
            //bitmap = new Bitmap(@"D:\Projects\Tracker\Tracker\snapshots\shot0004" + "" + ".png");
            //bitmap = new Bitmap(@"D:\Projects\RoverStudio\LEDs\20090405\Clipboard04.png");
            //Graphics g0 = Graphics.FromImage(bitmap);
            //g0.FillRectangle(Brushes.White, 0, 0, 640, 480);
            //g0.FillEllipse(Brushes.Black, 50, 50, 40, 40);

            Bitmap originalBitmap = (Bitmap)bitmap.Clone();

            if (this.overlayImage != null)
            {
                Subtract subtract = new Subtract(this.overlayImage);
                subtract.ApplyInPlace(bitmap);
            }

            Crop crop = new Crop(new Rectangle(this.Left, this.Top, this.Width, this.Height));
            Bitmap croppedImage = crop.Apply(bitmap);
            Grayscale grayscale = new Grayscale(0.4, 0.4, 0.2);
            Threshold threshold = new Threshold(this.threshold);
            Invert invert = new Invert();
            Bitmap grayImage = grayscale.Apply(croppedImage);
            Bitmap grayThresholdImage = threshold.Apply(grayImage);
            Bitmap grayInvertedThresholdImage = invert.Apply(grayThresholdImage);

            BlobCounter blobCounter = new BlobCounter();
            blobCounter.MaxHeight = maxBlobSize;
            blobCounter.MaxWidth = maxBlobSize;
            blobCounter.MinHeight = minBlobSize;
            blobCounter.MinWidth = minBlobSize;
            blobCounter.FilterBlobs = true;
            blobCounter.ProcessImage(grayInvertedThresholdImage);
            //blobCounter.ProcessImage(grayThresholdImage);

            Blob[] blobs = blobCounter.GetObjectInformation();

            int idx = -1;
            double minError = Double.MaxValue;

            Graphics g = Graphics.FromImage(bitmap);
            double meanX, meanY, moment;
            int pixelCount;
            for (int i = 0; i < blobs.Length; i++)
            {

                Bitmap blobImage = new Crop(blobs[i].Rectangle).Apply(grayInvertedThresholdImage);
                double r = GetRelativeTheta(blobImage, out meanX, out meanY, out pixelCount, out moment);
                if (Math.Abs(r - RELATIVE_THETA_CIRCLE) < minError)
                {
                    minError = Math.Abs(r - RELATIVE_THETA_CIRCLE);
                    idx = i;
                }
            }
            double x1=0, y1=0, x2=0, y2=0;

            if (idx >= 0)
            {
                g.DrawRectangle(Pens.Red, 
                    this.Left + blobs[idx].Rectangle.X,
                     this.Top + blobs[idx].Rectangle.Y,
                     blobs[idx].Rectangle.Width,
                     blobs[idx].Rectangle.Height);
                Bitmap insideTheCircle = new Crop(blobs[idx].Rectangle).Apply(croppedImage);
                insideTheCircle.Save(@"c:\inside.bmp");
                bitmap.Save(@"c:\bmp.bmp");
                double D = (blobs[idx].Rectangle.Width + blobs[idx].Rectangle.Height) / 2.0;
                double d = D * SMALL_LARGE_CIRCLE_RADIUS_RATIO * 0.8;

                int xRed, yRed, xGreen, yGreen;

                ImageStatistics redStat = new ImageStatistics(insideTheCircle);
                int redMean = (int)redStat.Red.Mean;
                Bitmap redInsideTheCircle = new ColorFiltering(
                    new AForge.IntRange(redMean, 255),
                    new AForge.IntRange(0, 255),
                    new AForge.IntRange(0, 255)).Apply(insideTheCircle);

                redInsideTheCircle.Save(@"c:\red.bmp");
                
                ImageStatistics greenStat = new ImageStatistics(insideTheCircle);
                int greenMean = (int)greenStat.Green.Mean;
                Bitmap greenInsideTheCircle = new ColorFiltering(
                    new AForge.IntRange(0, 255),
                    new AForge.IntRange(greenMean, 255),
                    new AForge.IntRange(0, 255)).Apply(insideTheCircle);

                greenInsideTheCircle.Save(@"c:\green.bmp");

                double pR = GetColorPeak(redInsideTheCircle, (int)Math.Ceiling(d), RGB.R, out xRed, out yRed);
                double pG = GetColorPeak(greenInsideTheCircle, (int)Math.Ceiling(d), RGB.G, out xGreen, out yGreen);

                if (pR < 0.3 && pG < 0.3)
                {
                    if (!this.lockSearchWindow)
                    {
                        this.Left -= 10;
                        this.Top -= 10;
                        this.Width += 20;
                        this.Height += 20;
                    }
                    Bitmap grayscaledFull0 = grayscale.Apply(bitmap);
                    filteredImages = new Bitmap[] { bitmap, grayscaledFull0, threshold.Apply(grayscaledFull0), grayInvertedThresholdImage };

                    x = 0;
                    y = 0;
                    theta = 0;
                    //                    counter++;


                    

                    return;
                }

                x1 = blobs[idx].Rectangle.Left + xRed;
                y1 = blobs[idx].Rectangle.Top + yRed;
                x2 = blobs[idx].Rectangle.Left + xGreen;
                y2 = blobs[idx].Rectangle.Top + yGreen;
                g.FillEllipse(Brushes.Red, this.Left + blobs[idx].Rectangle.Left + xRed - 5, this.Top + blobs[idx].Rectangle.Top + yRed - 5, 10, 10);
                g.FillEllipse(Brushes.Green, this.Left +  blobs[idx].Rectangle.Left + xGreen - 5, this.Top + blobs[idx].Rectangle.Top + yGreen - 5, 10, 10);

                //Bitmap closed = closing.Apply(insideTheCircle);
                //closed.Save(@"c:\closed.bmp");
                //BlobCounter innerBlobCounter = new BlobCounter();
                //innerBlobCounter.MaxHeight = Math.Min(blobs[idx].Rectangle.Width, blobs[idx].Rectangle.Height);
                //innerBlobCounter.MaxWidth = Math.Min(blobs[idx].Rectangle.Width, blobs[idx].Rectangle.Height);
                //innerBlobCounter.MinHeight = 1;
                //innerBlobCounter.MinWidth = 1;
                //innerBlobCounter.FilterBlobs = true;
                //innerBlobCounter.ProcessImage(insideTheCircle);
                //Blob[] innerBlobs = innerBlobCounter.GetObjectInformation();

                //int idx1 = -1;
                //double minError1 = Double.MaxValue;
                //int idx2 = -1;
                //double minError2 = Double.MaxValue;

                //for (int i = 0; i < innerBlobs.Length; i++)
                //{

                //    Bitmap blobImage = new Crop(innerBlobs[i].Rectangle).Apply(grayThresholdImage);
                //    double meanX, meanY, moment;
                //    int pixelCount;
                //    double r = GetRelativeTheta(blobImage, out meanX, out meanY, out pixelCount, out moment);
                //    if (Math.Abs(r - RELATIVE_THETA_CIRCLE) < minError1)
                //    {
                //        minError2 = minError1;
                //        idx2 = idx1;
                //        minError1 = Math.Abs(r - RELATIVE_THETA_CIRCLE);
                //        idx1 = i;
                //    }
                //    else if (Math.Abs(r - RELATIVE_THETA_CIRCLE) < minError2)
                //    {
                //        minError2 = Math.Abs(r - RELATIVE_THETA_CIRCLE);
                //        idx2 = i;
 
                //    }

                //}

                //g.DrawRectangle(Pens.Blue, blobs[idx].Rectangle.X + innerBlobs[idx1].Rectangle.X, 
                //    blobs[idx].Rectangle.Y + innerBlobs[idx1].Rectangle.Y,
                //    innerBlobs[idx1].Rectangle.Width,
                //    innerBlobs[idx1].Rectangle.Height);
                //g.DrawRectangle(Pens.Blue, blobs[idx].Rectangle.X + innerBlobs[idx2].Rectangle.X,
                //    blobs[idx].Rectangle.Y + innerBlobs[idx2].Rectangle.Y,
                //    innerBlobs[idx2].Rectangle.Width,
                //    innerBlobs[idx2].Rectangle.Height);

 
            }

            g.Dispose();

            Bitmap grayscaledFull = grayscale.Apply(bitmap);
            filteredImages = new Bitmap[] { bitmap, grayscaledFull, threshold.Apply(grayscaledFull), grayInvertedThresholdImage };
            bitmap.Save(@"C:\img.bmp");
            grayThresholdImage.Save(@"C:\img_g.bmp");
            grayInvertedThresholdImage.Save(@"C:\img_gi.bmp");
 //           counter++;
 //           grayInvertedThresholdImage.Save(@"D:\Projects\Tracker\Tracker\snapshots\fil_" + counter + ".bmp");

            double[] p1 = new double[]{x1,y1};
            double[] p2 = new double[]{x2,y2};
            theta = Math.Atan2(p2[1] - p1[1], p2[0] - p1[0]);
            x = (int)((p1[0] + p2[0]) / 2.0) + this.left;
            y = (int)((p1[1] + p2[1]) / 2.0) + this.top;
 
            //this.Width = Math.Max(30, (int)(4 * Math.Sqrt((p2[0] - p1[0]) * (p2[0] - p1[0]) + (p2[1] - p1[1]) * (p2[1] - p1[1]))) );
            //this.Height = Math.Max(30, (int)(4 * Math.Sqrt((p2[0] - p1[0]) * (p2[0] - p1[0]) + (p2[1] - p1[1]) * (p2[1] - p1[1]))) );
            if (!this.lockSearchWindow)
            {
                if (idx >= 0)
                {
                    this.Width = Math.Max(30, (int)(4 * blobs[idx].Rectangle.Width));
                    this.Height = Math.Max(30, (int)(4 * blobs[idx].Rectangle.Height));
                    this.Left = x - this.width / 2;
                    this.Top = y - this.height / 2;
                }
                else
                {
                    this.Left -= 10;
                    this.Top -= 10;
                    this.Width += 20;
                    this.Height += 20;
                }
            }
            //this.Left = 0;
            //this.Top = 0;
            //this.Width = 640;
            //this.Height = 480;

        }

        private double GetColorPeak(Bitmap image, int size, short color, out int peekX, out int peekY)
        {
            PixelFormat fmt = PixelFormat.Format24bppRgb;

            // lock bitmap data
            int width = image.Width;
            int height = image.Height;

            BitmapData imageData = image.LockBits(
                new Rectangle(0, 0, width, height),
                ImageLockMode.ReadOnly, fmt);



            double[,] rval = new double[width, height];
            double ravg = 0.0;

            int offset = imageData.Stride - width * 3;
            

            // do the job
            unsafe
            {
                byte* p = (byte*)imageData.Scan0.ToPointer();

                // for each line
                for (int y = 0; y < height; y++)
                {
                    // for each pixel
                    for (int x = 0; x < width; x++, p += 3)
                    {
                        int intensity = 0;
                        for (int i = 1; i < 3; i++)
                        {
                            if (i == RGB.B)
                                continue;
                            intensity += p[i];
                        }
                        if (intensity > 20 && (((double)(p[color])) / intensity > 0.5)) //20 is an ad-hoc threshold
                            rval[x, y] = ((double)(p[color])) / intensity;
                        else
                            rval[x, y] = 0.0;
                        ravg += rval[x, y];
                     }
                    p += offset;
                }
            }


            // unlock image
            image.UnlockBits(imageData);

            int r = size / 2;

            double maxColorSum = 0.0;

            ravg /= (width * height);

            for (int x = r; x < width - r; x++)
            {
                for (int y = r; y < height - r; y++)
                {
                    if (rval[x, y] < ravg)
                        rval[x, y] = 0.0;
                }
            }
            peekX = peekY = 0;

            for (int x = r; x < width - r; x++)
            {
                for (int y = r; y < height - r; y++)
                {
                    double colorSum = 0.0;
                    for (int i = -r; i <= r; i++)
                    {
                        for (int j = -r; j <= r; j++)
                        {
                            colorSum += rval[x + i, y + j];
                        }
                    }

                    if (colorSum > maxColorSum)
                    {
                        peekX = x;
                        peekY = y;
                        maxColorSum = colorSum;
                    }
                }
            }

            return maxColorSum / ( size * size);
        }

        private double GetRelativeTheta(Bitmap image, out double meanX, out double meanY, out int pixelCount, out double theta)
        {
            meanX = 0.0;
            meanY = 0.0;

            pixelCount = 0;

            theta = 0.0;

            PixelFormat fmt = PixelFormat.Format8bppIndexed;

            // lock bitmap data
            int width = image.Width;
            int height = image.Height;

            BitmapData imageData = image.LockBits(
                new Rectangle(0, 0, width, height),
                ImageLockMode.ReadOnly, fmt);




            byte rv, gv, bv;

            int offset = imageData.Stride - width * 1;

            // do the job
            unsafe
            {
                byte* p = (byte*)imageData.Scan0.ToPointer();

                // for each line
                for (int y = 0; y < height; y++)
                {
                    // for each pixel
                    for (int x = 0; x < width; x++, p++)
                    {


                        if (*p > 0)   //If pixel is not black
                        {
                            meanX += x;
                            meanY += y;
                            theta += x * x + y * y;
                            pixelCount++;
                        }
                     }
                    p += offset;
                }
            }


            // unlock image
            image.UnlockBits(imageData);

            if (pixelCount > 0)
            {
                meanX /= pixelCount;
                meanY /= pixelCount;
                theta = theta - (meanX * meanX + meanY * meanY) * pixelCount;
                return theta / (pixelCount * pixelCount);

            }

            return 0.0;
            
        }


 
    }
}
